#include <iostream>
#include <limits>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/interactor_style.h>
#include <pcl/visualization/point_picking_event.h>
#include <Utils.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkRenderWindow.h>
#include "../plane_info.h"
using namespace std;
using namespace lyxutils::abbreviations;

typedef pcl::PointXYZ PT;
typedef pcl::PointCloud<PT> CloudT;
typedef pcl::PointXYZRGB PTRGB;
typedef pcl::PointCloud<PTRGB> CloudRGB;
typedef CloudT::Ptr CloudPtr;
typedef pcl::visualization::PCLVisualizer Viewer;

//global var for coordinate of system original
Vec3f origin_pos={FLT_MAX,FLT_MAX,FLT_MAX};

void printUsage(){
    cout<<"usage:visual_planes <planes.txt> [points.pcd|folder...]"<<endl;
    cout<<"      visualize planes [along with point cloud]"<<endl;
    cout<<"      planes.txt         =>the file that contains plane information,"<<endl;
    cout<<"                           columns:x,y,z coordinates of center,xAxis,"<<endl;
    cout<<"                           yAXis,zAxis,sigma,maxXYZ,minXYZ,ptCount"<<endl;
    cout<<"      points.pcd|folder  =>the point cloud file or folder that contains"<<endl;
    cout<<"                           single|multiple point cloud(s)"<<endl;
    cout<<"arguments:"<<endl;
    cout<<"  -h           print this help message"<<endl;
    cout<<"  -b=r,g,b     set background color(default is black)"<<endl;
    cout<<"  -c[=scale]   enable coordinate system display(and set coordinate scale,default is 50.0)"<<endl;
    cout<<"  -p=r,g,b     set point color(default is white).if -p=random(or -p=r),the point"<<endl;
    cout<<"               cloud color will be assigned randomly;if -p=origin(or -p=o),the points"<<endl;
    cout<<"               will be rendered original color"<<endl;
    cout<<"  -f=r,g,b     set plane wire-frame color as `color`(default is green)"<<endl;
    cout<<"  -n=r,g,b     set normal vector color as `color`(default is red)"<<endl;
    cout<<"  -s=size      set point size(default is 3)"<<endl;
    cout<<"  -l=len       set normal vector length(default is 5)"<<endl;
    cout<<"  -a=file.txt  append a plane information file(used in match visualization)"<<endl;
    cout<<"  -m=match.txt set match result file(used in match result visualization)"<<endl;
    cout<<"               note:-m option will only be effective when -a option is set,each row"<<endl;
    cout<<"               in match.txt contains at least two ints, the 1st one represents index of"<<endl;
    cout<<"               plane from planes.txt while the 2nd one represents the corresponding one"<<endl;
    cout<<"               from file.txt(index out of range will be ignored)"<<endl;
    cout<<"  -e           draw the evidence supports the correspondence of the resultant match."<<endl;
    cout<<"               this option is only effective when -m is set"<<endl;
    cout<<"  -t=n         set draw matches interval as n(meaning draw lines for every n+1 matches,"<<endl;
    cout<<"               default n is 0).this option is used for sparsification of matched planes"<<endl;
    cout<<"  -i           enable interactive mode"<<endl;
    cout<<"  -o           only draw matched planes"<<endl;
}

template<typename PointT,typename allocator=std::allocator<PointT>()>
void getMinMax(const vector<PointT,allocator> &pList,Vec3f &min){
    if(pList.size()==0)return;
    min=Vec3f(pList[0].x,pList[0].y,pList[0].z);
    for(const auto &pt:pList){
        if(pt.x<min.x)min.x=pt.x;
        if(pt.y<min.y)min.y=pt.y;
        if(pt.z<min.z)min.z=pt.z;
    }
}

void loadXYZCloud(string filename,CloudT &cloud){
    if(xos::suffix(filename)=="pcd"){
        pcl::io::loadPCDFile(filename,cloud);
    }
    else if(xos::suffix(filename)=="txt"){
        xio::read_pts(filename,cloud.points,",");
        cloud.width=cloud.points.size();
        cloud.height=1;
        cloud.is_dense=true;
    }
}

void loadRGBCloud(string filename,CloudRGB &cloud){
    if(xos::suffix(filename)=="pcd"){
        pcl::io::loadPCDFile(filename,cloud);
    }
    else if(xos::suffix(filename)=="txt"){
        xio::read_ptsrgb(filename,cloud.points,",");
        cloud.width=cloud.points.size();
        cloud.height=1;
        cloud.is_dense=true;
    }
}

void addPointCloud(string filename,Viewer::Ptr viewer,Vec3i color,int size,bool keep_color=false){
    string cloud_name=xos::abspath(filename);
    Vec3f pt;
    if(keep_color){//read colorized point cloud:PointCloud<PointXYZRGB>
        CloudRGB::Ptr cloud(new CloudRGB);
//        pcl::io::loadPCDFile(filename,*cloud);
        loadRGBCloud(filename,*cloud);
        viewer->addPointCloud(cloud,cloud_name);
        getMinMax(cloud->points,pt);
    }
    else{//read plain point cloud and assign color
        CloudPtr cloud(new CloudT);
//        pcl::io::loadPCDFile(filename,*cloud);
        loadXYZCloud(filename,*cloud);
        pcl::visualization::PointCloudColorHandlerCustom<PT> single_color(cloud,color.x,color.y,color.z);
        viewer->addPointCloud<PT>(cloud,single_color,cloud_name);
        getMinMax(cloud->points,pt);
    }
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,size,cloud_name);
    //get minimum coords
    if(origin_pos.x>pt.x)origin_pos.x=pt.x;
    if(origin_pos.y>pt.y)origin_pos.y=pt.y;
    if(origin_pos.z>pt.z)origin_pos.z=pt.z;
}

void addPlane(const PlaneInfo &plane,Viewer::Ptr viewer,string plane_name,
                Vec3i frame_color, Vec3i normal_color,float normal_len){
    vector<Vec3f> pts;
    plane.getVertices(pts);
    vector<PT> points;
    for(auto p:pts)points.push_back(PT(p.x,p.y,p.z));
    auto add_edge=[&](int idx){
        string line_id=plane_name+"_"+str::type2str(idx);
        viewer->addLine(points[idx%4],points[(idx+1)%4],frame_color.x,frame_color.y,frame_color.z,line_id);
    };
    add_edge(0);
    add_edge(1);
    add_edge(2);
    add_edge(3);
    string line_id=plane_name+"_"+str::type2str(4);
    viewer->addLine(points[0],points[2],frame_color.x,frame_color.y,frame_color.z,line_id);
    line_id=plane_name+"_"+str::type2str(5);
    viewer->addLine(points[1],points[3],frame_color.x,frame_color.y,frame_color.z,line_id);
    Vec3f center=np::mean(pts);
    Vec3f tip=center+normal_len*plane.zAxis;
    PT normal1(center.x,center.y,center.z);
    PT normal2(tip.x,tip.y,tip.z);
    line_id=plane_name+"_normal";
    viewer->addLine(normal1,normal2,normal_color.x,normal_color.y,normal_color.z,line_id);
}

//get center point of a plane(mean coordinate of vertices)
PT getCenterPt(const PlaneInfo &plane){
    vector<Vec3f> vertices;
    plane.getVertices(vertices);
    Vec3f center=np::mean(vertices);
    return {center.x,center.y,center.z};
}

void drawMatches(string filename,Viewer::Ptr viewer,Vec3i background,
        const vector<PlaneInfo> &planes1,const vector<PlaneInfo> planes2, bool drawEvidence,int interval,bool drawPlane=false){
    xcolor::ColorGenerator matchGen(background);
    xcolor::ColorGenerator relGen(background);
    vector<vector<int> > matchIdx;
    //read matches from file
    xio::read_csv_customize(filename,[&matchIdx](const vector<float> &row){
        matchIdx.push_back(vector<int>());
        for(float f:row)matchIdx[matchIdx.size()-1].push_back(int(f));
    },",");
    //draw match lines and corresponding matched spacial relations
    for(int i=0;i<matchIdx.size();++i){
        if(i%(interval+1)!=0)continue;
        if(matchIdx[i][0]<0||matchIdx[i][0]>=planes1.size())continue;
        if(matchIdx[i][1]<0||matchIdx[i][1]>=planes2.size())continue;
        const PlaneInfo &p1=planes1[matchIdx[i][0]];
        const PlaneInfo &p2=planes2[matchIdx[i][1]];
        PT pt11=getCenterPt(p1);
        PT pt21=getCenterPt(p2);
        //I have no fucking clue why the color need to be converted to 0~1 as double
        //I used integer color(0~255) just fine in addPointCloud and addPlanes
        Vec3d matchColor=matchGen().asType<double>()/255;
        string namePrefix=filename+str::type2str(i);
        viewer->addLine(pt11,pt21,matchColor.x,matchColor.y,matchColor.z,namePrefix);
        if(drawEvidence){
            for(int j=2;j<matchIdx[i].size()-1;j+=2){
                const PlaneInfo &p12=planes1[matchIdx[i][j]];
                const PlaneInfo &p22=planes2[matchIdx[i][j+1]];
                PT pt12=getCenterPt(p12);
                PT pt22=getCenterPt(p22);
                Vec3d relColor=relGen().asType<double>()/255;
                viewer->addLine(pt11,pt12,relColor.x,relColor.y,relColor.z,namePrefix+"_"+str::type2str(j));
                viewer->addLine(pt21,pt22,relColor.x,relColor.y,relColor.z,namePrefix+"_"+str::type2str(j+1));
            }
        }
        if(drawPlane){
            Vec3i plane_color=(matchColor*255).asType<int>();
            addPlane(p1,viewer,"plane_"+str::type2str(i)+"_ref",plane_color,plane_color,2);
            addPlane(p2,viewer,"plane_"+str::type2str(i)+"_align",plane_color,plane_color,2);
        }
    }
}

bool iteration_flag=true;//event loop flag when interactive mode is enabled

//current working mode for interactor events handle
enum class Mode{
    normal,
    selection
};

struct EventCallbackCookie{
    //viewer to draw
    Viewer::Ptr viewer;
    //command line foreground color
    Vec3i background;
    //plane info to add
    vector<PlaneInfo> planes1;
    vector<PlaneInfo> planes2;
    Vec3i frameColor;
    Vec3i normalColor;
    float normLength;
    //match info to add
    vector<vector<int> > matches;
    bool drawEvidence;
    vector<float> match_scores;
    vector<vector<float> > relation_scores;//scores of relation features of each pair of matched plane
    //this contains random color used to draw matches,the colors will be generated when necessary
    vector<Vec3d> matchColors;
    //current state flags
    Mode state=Mode::normal;
}data;//event callback args

class StatusCmd{
public:
    StatusCmd():data(1,""),inputIdx(1,0),cmd_cursor(0),outPrefix("Out:"){}
    void push_back(string str,bool output=false){
        if(output)str=outPrefix+str;
        if(data[data.size()-1]==""){//in-place modify
            data[data.size()-1]=str;
            //last one has been changed from input to output
            if(output&&inputIdx[inputIdx.size()-1]==data.size()-1)inputIdx.pop_back();
        }
        else{//append to data
            data.push_back(str);
            //last one is not output
            if(!output)inputIdx.push_back(data.size()-1);
        }
        cmd_cursor=inputIdx.size()-1;
        if(output){
            data.push_back("");//add a new line
            inputIdx.push_back(data.size()-1);//assume it's input line
        }
    }
    void append(char ch){data[data.size()-1].push_back(ch);}
    void backspace(){
        if(data[data.size()-1].size()>0)data[data.size()-1].pop_back();
    }
    bool isOutput(uint idx){
        if(data[idx].size()>=outPrefix.size()&&data[idx].substr(0,outPrefix.size())==outPrefix)return true;
        return false;
    }
    void setOutPrefix(string prefix){
        if(prefix.size()==0)return;//illegal prefix will not be used
        for(int i=0;i<data.size();++i){
            if(isOutput(i))data[i]=prefix+data[i].substr(outPrefix.size(),data[i].size()-outPrefix.size());
        }
        outPrefix=prefix;
    }
    string& operator[](uint idx){return data[idx];}
    //set last element and not changing cursor at the same time
    void setLast(string str){
        data[data.size()-1]=str;
    }
    string last(){return data[data.size()-1];}
    string printStringAt(uint idx){
        if(isOutput(idx))return data[idx];
        return "In $"+data[idx];
    }
    string previous(){
        if(cmd_cursor>0)cmd_cursor--;
        return data[inputIdx[cmd_cursor]];
    }
    string next(){
        if(cmd_cursor<inputIdx.size())cmd_cursor++;
        if(cmd_cursor==inputIdx.size())return "";
        else return data[inputIdx[cmd_cursor]];
    }
    uint size(){return data.size();}
private:
    vector<string> data;
    vector<int> inputIdx;
    int cmd_cursor;
    string outPrefix;
};
//#1,forward declaration to avoid error at spot #2
class MyInteractorStyle;
//#2,this global variable need to proceed the definition of MyInteractorStyle::Initialize at spot #5
std::shared_ptr<MyInteractorStyle> style;
//#3,forward declaration before connect it to the signal at spot #7
void pp_callback(const pcl::visualization::PointPickingEvent& event);
//#4, I need to customize the interactor style cause the default version has
// some conflict keyboard event callback
class MyInteractorStyle:public pcl::visualization::PCLVisualizerInteractorStyle{
public:
    static MyInteractorStyle *New();
    //#5,customize Initialize function to do some customized initialization
    void Initialize(){
        PCLVisualizerInteractorStyle::Initialize();
        //attributes initialization
        num_rows=5;
        current_match_index=0;
        relation_feature_limit=10;
        point_picking_signal_.connect(pp_callback);//#7
    }
    void showCommands(){
        //print last num_rows commands
        Viewer::Ptr viewer=data.viewer;
        Vec3d forecolor=(255-data.background).asType<double>()/255.0;
        int fontSize=14;
        int space=4;
        int xpos=50;
        int ypos=num_rows*(fontSize+space)+20;
        for(int i=0;i<num_rows;++i){
            int idx= commands.size() - num_rows + i;
            if(idx>=0){
                string cmd=commands.printStringAt(idx);
                string id="command"+i;
                if(viewer->contains(id))viewer->removeShape(id);
                viewer->addText(cmd,xpos,ypos-i*(fontSize+space),fontSize,forecolor.x,forecolor.y,forecolor.z,id);
            }
        }
    }
    friend void pp_callback(const pcl::visualization::PointPickingEvent &event);
protected:
    void OnKeyDown (){
//        cout<<"current key code:"<<(int)Interactor->GetKeyCode()<<endl;//test key code
//        cout<<"current key symbol:"<<Interactor->GetKeySym()<<endl;//test key symbol
        if(Interactor->GetKeyCode()==13){//enter button pressed
            //process commands
            if(commands.last() != "" && !commands.isOutput(commands.size() - 1))process_commands(commands.last());
            //add next line
            commands.push_back("");
        }
        else if(Interactor->GetKeyCode()==8){//backspace
            commands.backspace();
        }
        else if(string(Interactor->GetKeySym())=="Escape"){
            interactor_exit();
        }
        else if(string(Interactor->GetKeySym())=="Up"){
            if(data.state==Mode::normal)commands.setLast(commands.previous());//previous command
            else if(data.state==Mode::selection){//previous match
                int idx=current_match_index-1;
                if(idx<0)idx=0;
                if(idx!=current_match_index){
                    select_match(idx);
                }
            }
        }
        else if(string(Interactor->GetKeySym())=="Left"){
            if(data.state==Mode::selection){
                int prev_limit=relation_feature_limit;
                relation_feature_limit--;
                if(relation_feature_limit<0)relation_feature_limit=0;
                if(relation_feature_limit!=prev_limit){
                    select_match(current_match_index);
                }
            }
        }
        else if(string(Interactor->GetKeySym())=="Down"){
            if(data.state==Mode::normal)commands.setLast(commands.next());//next command
            else if(data.state==Mode::selection){//next match
                int idx=current_match_index+1;
                if(idx>=data.matches.size())idx=data.matches.size()-1;
                if(idx!=current_match_index){
                    select_match(idx);
                }
            }
        }
        else if(string(Interactor->GetKeySym())=="Right"){
            if(data.state==Mode::selection){
                int prev_limit=relation_feature_limit;
                relation_feature_limit++;
                if(relation_feature_limit>(data.matches[current_match_index].size()-2)/2){
                    relation_feature_limit=(data.matches[current_match_index].size()-2)/2;
                }
                if(relation_feature_limit!=prev_limit){
                    select_match(current_match_index);
                }
            }
        }
        else if(Interactor->GetKeyCode()>31&&Interactor->GetKeyCode()<128){
            //process key with modifiers here

            //printable character pressed(without any key modifier such as ctrl,shift,alt
            commands.append(Interactor->GetKeyCode());
        }
        showCommands();
    }
private:
    void process_commands(string cmd){
        if(data.state==Mode::normal){
            if(cmd=="exit"||cmd=="quit"){
                interactor_exit();
            }
            else if(cmd=="select"){
                data.state=Mode::selection;
                commands.push_back("enter selection mode(press Esc back to normal mode)", true);
                select_match(current_match_index);
            }
        }
        else if(data.state==Mode::selection){//selection command
            try{
                int idx=stoi(cmd);
                //index range test here
                if(idx<0||idx>=data.matches.size())throw out_of_range("index of match pairs out of range!total number of pairs:"+str::type2str(data.matches.size()));
                select_match(idx);
            }
            catch(out_of_range ofr){
                commands.push_back(ofr.what(),true);
            }
            catch(...){//catch anything that goes wrong
                commands.push_back("unrecognized command:"+cmd+",current mode:selection mode",true);
            }
        }
    }
    void interactor_exit(){
        if(data.state==Mode::normal)iteration_flag=false;//interactor_exit event loop
        else{
            data.state=Mode::normal;
            commands.push_back("back to normal mode", true);
        }
    }
    void select_match(int idx){
        cancel_draw();
        if(data.matches.size()==0){
            commands.push_back("there is no match result available!",true);
            return;
        }
        draw_match(idx);
        current_match_index=idx;
        string outStr="current match pair index #"+str::type2str(idx);
        outStr+=",plane index from source1 #"+str::type2str(data.matches[idx][0]);
        outStr+=",plane index from source2 #"+str::type2str(data.matches[idx][1]);
        outStr+=",score="+str::type2str(data.match_scores[idx]);
        //only output relation feature scores when necessary (data.drawEvidence) and available
        if(data.drawEvidence&&actual_display_feature<=data.relation_scores[idx].size()){
            outStr+=",first several scores=";
            if(actual_display_feature<=6){
                if(actual_display_feature>0)outStr+=str::type2str(data.relation_scores[idx][0]);
                for(int i=1;i<actual_display_feature;++i){//print out each relation feature match score
                    outStr+=","+str::type2str(data.relation_scores[idx][i]);
                }
            }
            else{
                for(int i=0;i<3;++i)outStr+=str::type2str(data.relation_scores[idx][i])+",";
                outStr+="...,"+str::type2str(data.relation_scores[idx][actual_display_feature-2]);
                outStr+=","+str::type2str(data.relation_scores[idx][actual_display_feature-1]);
            }
        }
        commands.push_back(outStr,true);
    }
    void select_plane_only(int group,int idx){
        cancel_draw();
        string outStr="plane #"+str::type2str(idx)+" from point cloud #"+str::type2str(group+1);
        outStr+=",no match";
        commands.push_back(outStr,true);
        if(group==0){
            addPlane(data.planes1[idx],data.viewer,"tmp_plane1",data.frameColor,data.normalColor,data.normLength);
            for(int j=0;j<6;++j){
                current_draw.push_back("tmp_plane1_"+str::type2str(j));
            }
            current_draw.push_back("tmp_plane1_normal");
        }
        else{
            addPlane(data.planes2[idx],data.viewer,"tmp_plane2",data.frameColor,data.normalColor,data.normLength);
            for(int j=0;j<6;++j){
                current_draw.push_back("tmp_plane2_"+str::type2str(j));
            }
            current_draw.push_back("tmp_plane2_normal");
        }
    }
    void draw_point(const PT &pt){
        CloudT::Ptr cloud(new CloudT);
        cloud->points.push_back(pt);
        pcl::visualization::PointCloudColorHandlerCustom<PT> red(cloud, 255, 0, 0);
        data.viewer->removePointCloud("clicked_points");
        data.viewer->addPointCloud(cloud, red, "clicked_points");
        data.viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
        std::cout << "clicked point:" << pt.x << "," << pt.y << "," << pt.z << std::endl;
    }
    void draw_match(int currentIdx){
        int id1=data.matches[currentIdx][0];
        int id2=data.matches[currentIdx][1];
        if(id1<0||id1>=data.planes1.size()||id2<0||id2>=data.planes2.size())return;
        const PlaneInfo &plane1=data.planes1[id1];
        const PlaneInfo &plane2=data.planes2[id2];
        //added shapes:tmp_plane1_0,...tmp_plane1_5,tmp_plane1_normal
        addPlane(plane1,data.viewer,"tmp_plane1",data.frameColor,data.normalColor,data.normLength);
        //added shapes:tmp_plane2_0,...tmp_plane2_5,tmp_plane2_normal
        addPlane(plane2,data.viewer,"tmp_plane2",data.frameColor,data.normalColor,data.normLength);
        for(int i=1;i<=2;++i){
            for(int j=0;j<6;++j){
                current_draw.push_back("tmp_plane"+str::type2str(i)+"_"+str::type2str(j));
            }
            current_draw.push_back("tmp_plane"+str::type2str(i)+"_normal");
        }
        xcolor::ColorGenerator gen(data.background);
        PT pt1=getCenterPt(plane1);
        PT pt2=getCenterPt(plane2);
        if(data.matchColors.size()==0){
            data.matchColors.emplace_back(gen().asType<double>()/255.0);
        }
        Vec3d mColor=data.matchColors[0];
        data.viewer->addLine(pt1,pt2,mColor.x,mColor.y,mColor.z,"tmp_match");
        current_draw.push_back("tmp_match");
        if(data.drawEvidence){
            actual_display_feature=0;
            for(int j=2;j<data.matches[currentIdx].size()-1&&j<2+relation_feature_limit*2;j+=2){
                int id11=data.matches[currentIdx][j];
                int id22=data.matches[currentIdx][j+1];
                if(id11<0||id11>=data.planes1.size()||id22<0||id22>=data.planes2.size())continue;
                const PlaneInfo &tmp1=data.planes1[id11];
                const PlaneInfo &tmp2=data.planes2[id22];
                PT pt11=getCenterPt(tmp1);
                PT pt22=getCenterPt(tmp2);
                while(data.matchColors.size()<actual_display_feature+2){
                    data.matchColors.emplace_back(gen().asType<double>()/255.0);
                }
                Vec3d eviColor=data.matchColors[1+actual_display_feature];
                //added shapes:tmp_match_j,tmp_match_{j+1}
                data.viewer->addLine(pt1,pt11,eviColor.x,eviColor.y,eviColor.z,"tmp_match_"+str::type2str(j));
                data.viewer->addLine(pt2,pt22,eviColor.x,eviColor.y,eviColor.z,"tmp_match_"+str::type2str(j+1));
                current_draw.push_back("tmp_match_"+str::type2str(j));
                current_draw.push_back("tmp_match_"+str::type2str(j+1));
                //add shapes:tmp_support_{j~j+1}_{0~5},tmp_support_{j~j+1}_normal
                addPlane(tmp1,data.viewer,"tmp_support_"+str::type2str(j),data.frameColor,data.normalColor,data.normLength);
                addPlane(tmp2,data.viewer,"tmp_support_"+str::type2str(j+1),data.frameColor,data.normalColor,data.normLength);
                for(int k=0;k<6;++k){
                    current_draw.push_back("tmp_support_"+str::type2str(j)+"_"+str::type2str(k));
                    current_draw.push_back("tmp_support_"+str::type2str(j+1)+"_"+str::type2str(k));
                }
                current_draw.push_back("tmp_support_"+str::type2str(j)+"_normal");
                current_draw.push_back("tmp_support_"+str::type2str(j+1)+"_normal");
                actual_display_feature++;
            }
        }
    }
    void cancel_draw(){
        string name;
        auto removeShape=[&](string name){
            if(data.viewer->contains(name))data.viewer->removeShape(name);
        };
        for(int i=0;i<current_draw.size();++i){
            removeShape(current_draw[i]);
        }
        current_draw.clear();
    }
    //private variables
    StatusCmd commands;
    int num_rows;//number of command rows to print
    int current_match_index;//current match to show
    int relation_feature_limit;//current limit on number of relation features to display
    int actual_display_feature;//actually displayed number of relation features
    vector<string> current_draw;//record id of current drawn objects in order for later deletion by id
};

vtkStandardNewMacro(MyInteractorStyle)

void find_plane(const PT &pt,int &group,int &idx){
    auto within=[](const PlaneInfo &plane,const Vec3f &pt)->bool{
        Vec3f cen2pt=pt-plane.center;
        float x=cen2pt.dot(plane.xAxis);
        float y=cen2pt.dot(plane.yAxis);
        float z=cen2pt.dot(plane.zAxis);
        Vec3f min=plane.minXYZ;
        Vec3f max=plane.maxXYZ;
        if(x>min.x&&x<max.x&&y>min.y&&y<max.y&&z>min.z&&z<max.z)return true;
        return false;
    };
    Vec3f pnt(pt.x,pt.y,pt.z);
    for(int i=0;i<data.planes1.size();++i){
        if(within(data.planes1[i],pnt)){
            group=0;
            idx=i;
            return;
        }
    }
    for(int i=0;i<data.planes2.size();++i){
        if(within(data.planes2[i],pnt)){
            group=1;
            idx=i;
            return;
        }
    }
    group=-1;
    idx=-1;
}
//group:0-planes from point cloud1;1-planes from point cloud2
int find_match(int group,int idx){
    if(data.matches.size()==0)return -1;
    for(int i=0;i<data.matches.size();++i){
        if(data.matches[i][group]==idx)return i;
    }
    return -1;
}

void pp_callback(const pcl::visualization::PointPickingEvent& event){
    if(data.state==Mode::selection&&style->Interactor->GetControlKey()){
        //ctrl+shift+left button will cause point picking event
        PT pt;
        event.getPoint(pt.x,pt.y,pt.z);
        int group,idx;
        int mIdx;
        find_plane(pt,group,idx);
        if(group!=-1&&idx!=-1){
            mIdx=find_match(group,idx);
            if(mIdx!=-1){
                //select the match that contains the mouse selected plane
                style->select_match(mIdx);
            }
            else{
                //only draw the selected plane,there is no match for this plane
                style->select_plane_only(group,idx);
            }
            style->draw_point(pt);//draw the selected point
        }
    }
}

int main(int argc,char **argv) {
    xio::CLParser parser;
    parser.parse(argc,argv);
    vector<string> paras=parser.getParameters();
    if(paras.size()==0 || parser.hasOption("h")){
        printUsage();
        return 1;
    }
    if(!xos::file(xos::abspath(paras[0]))){
        cout<<"file:"<<paras[0]<<" not exist!"<<endl;
        return 1;
    }
    //deal with options
    Vec3i background={0,0,0};
    if(parser.hasOption("b")){
        background.fromStringVector(str::split(parser.getOptionArg("b"),","));
    }
    float scale=parser.getOptionArg("c")!=""?stof(parser.getOptionArg("c")):50.0;
    Vec3i point_color={255,255,255};
    bool rand_color=false;
    bool origin_color=false;
    if(parser.hasOption("p")){
        string args=parser.getOptionArg("p");
        if(args=="random"||args=="r"){
            rand_color=true;
        }
        else if(args=="origin"||args=="o"){
            origin_color=true;
        }
        else{
            point_color.fromStringVector(str::split(args,","));
        }
    }
    Vec3i frame_color={0, 255, 0};
    if(parser.hasOption("f")){
        frame_color.fromStringVector(str::split(parser.getOptionArg("f"), ","));
    }
    Vec3i normal_color={255,0,0};
    if(parser.hasOption("n")){
        normal_color.fromStringVector(str::split(parser.getOptionArg("n"),","));
    }
    int point_size=parser.hasOption("s")?stoi(parser.getOptionArg("s")):3;
    float n_length=parser.hasOption("l")?stof(parser.getOptionArg("l")):5.0;
    string appdFile=parser.getOptionArg("a");
    string matchFile=parser.getOptionArg("m");
    //set title of the window
    string title=xos::abspath(paras[0]);
    if(paras.size()>1){
        title+="+"+xos::relpath(paras[1],xos::abspath(paras[0]));
    }
    if(paras.size()>2){
        title+="...";
    }
    int max_title_len=80;
    if(title.size()>max_title_len)title="..."+title.substr(title.size()-max_title_len,max_title_len);
    //visualization
    Viewer::Ptr viewer(new Viewer(title,false));
    viewer->setBackgroundColor(background.x,background.y,background.z);
    //draw planes
    vector<PlaneInfo> planes;
    ifstream ifs(xos::abspath(paras[0]));
    string line;
    while(getline(ifs,line)){
        if(line=="")continue;
        planes.push_back(PlaneInfo());
        planes[planes.size()-1].parseLine(line);
    }
    ifs.close();
    cout<<"adding planes(total number of added planes:"<<planes.size()<<")..."<<endl;
    if(parser.hasOption("i")){//interactive mode
        data.frameColor=frame_color;
        data.normalColor=normal_color;
        data.normLength=n_length;
        for(int i=0;i<planes.size();++i){
            data.planes1.push_back(planes[i]);
        }
    }
    else if(!parser.hasOption("o")){//non-interactive mode
        for(int i=0;i<planes.size();++i){
            addPlane(planes[i],viewer,xos::abspath(paras[0])+str::type2str(i),frame_color,normal_color,n_length);
        }
    }
    //draw point clouds
    for(int i=1;i<paras.size();++i){
        string filename=xos::abspath(paras[i]);
        if(xos::directory(filename)){
            vector<string> files=xos::lsfile(filename);
            xcolor::ColorGenerator gen;
            int deny_range=256/(files.size()+1);
            deny_range=deny_range<10?10:deny_range;
            gen=xcolor::ColorGenerator(background,deny_range/2);
            int display_count=1;
            cout<<"adding point cloud..."<<endl;
            for(string f:files){
                if(rand_color)point_color=gen();
                addPointCloud(f,viewer,point_color,point_size,origin_color);
                xio::consoleProgressBar(display_count++,files.size());
            }
            if(display_count<=files.size())xio::consoleProgressBar(100,100);
        }
        else if(xos::file(filename)){
            xcolor::ColorGenerator gen(background,64);
            if(rand_color)point_color=gen();
            addPointCloud(filename,viewer,point_color,point_size,origin_color);
        }
        else{
            cout<<"file or directory:"<<paras[1]<<" not exist!=>omitted"<<endl;
        }
    }
    //load and draw planes from another point cloud source
    vector<PlaneInfo> planes2;
    if(parser.hasOption("a")){
        string filename=xos::abspath(parser.getOptionArg("a"));
        if(!xos::file(filename)){
            cout<<"file:"<<parser.getOptionArg("a")<<" not exist!"<<endl;
            cout<<"this error is omitted and -a,-m options are automatically disabled"<<endl;
        }
        else{
            ifstream ifs2(filename);
            string line;
            while(getline(ifs2,line)){
                if(line=="")continue;
                planes2.push_back(PlaneInfo());
                planes2[planes2.size()-1].parseLine(line);
            }
            ifs2.close();
            cout<<"adding planes(total number of added planes:"<<planes2.size()<<")"<<endl;
            if(parser.hasOption("i")){//interactive mode
                data.frameColor=frame_color;
                data.normalColor=normal_color;
                data.normLength=n_length;
                for(int i=0;i<planes2.size();++i){
                    data.planes2.push_back(planes2[i]);
                }
            }
            else if(!parser.hasOption("o")){//non-interactive mode
                for(int i=0;i<planes2.size();++i){
                    addPlane(planes2[i],viewer,filename+str::type2str(i),frame_color,normal_color,n_length);
                }
            }
        }
    }
    //draw plane matches
    if(parser.hasOption("m") && planes2.size()>0){
        string filename=xos::abspath(parser.getOptionArg("m"));
        if(!xos::file(filename)){
            cout<<"file:"<<parser.getOptionArg("m")<<" not exist!matches will not be drawn"<<endl;
        }
        else{
            cout<<"draw matches";
            if(parser.hasOption("e"))cout<<"(with supporting relation vector)";
            cout<<endl;
            int interval=parser.hasOption("t")?stoi(parser.getOptionArg("t")):0;
            if(parser.hasOption("i")){//interactive mode
                xio::read_csv_customize(filename,[](const vector<float> row){
                    data.matches.push_back(vector<int>());
                    for(const float& val:row)data.matches[data.matches.size()-1].push_back((int)val);
                },",");
                data.drawEvidence=parser.hasOption("e");
                //read match scores file if exist
                filename=xos::parent(filename)+"/top_match_scores.txt";
                if(xos::file(filename)){
                    vector<vector<float> > top_scores;
                    xio::read_csv(filename,top_scores,",");
                    data.match_scores.clear();
                    for(int i=0;i<data.matches.size();++i){
                        data.match_scores.push_back(top_scores[i][2]);
                        data.relation_scores.emplace_back();
                        for(int j=3;j<top_scores[i].size();++j){
                            data.relation_scores[i].push_back(top_scores[i][j]);
                        }
                    }
                }
            }
            else{//non-interactive mode
                drawMatches(filename,viewer,background,planes,planes2,parser.hasOption("e"),interval,parser.hasOption("o"));
            }
        }
    }
    if(parser.hasOption("c"))viewer->addCoordinateSystem(scale,origin_pos.x-scale,origin_pos.y-scale,origin_pos.z);
    viewer->setShowFPS(true);
    if(parser.hasOption("i")){
        //set keyboard event callback extra parameters
        data.viewer=viewer;
        data.background=background;
        //setup my custom interactor style
        viewer->createInteractor();
        auto interactor=viewer->getInteractorStyle()->GetInteractor();
//        auto interactorStyle=MyInteractorStyle::New();
        style=std::shared_ptr<MyInteractorStyle>(MyInteractorStyle::New());
        auto renderCollection=viewer->getRendererCollection();
//        interactorStyle->Initialize();
//        interactorStyle->setRendererCollection(renderCollection);
        style->Initialize();
        style->setRendererCollection(renderCollection);
//        interactor->SetInteractorStyle(interactorStyle);
        interactor->SetInteractorStyle(style.get());
        interactor->Initialize();
//        viewer->setupInteractor(interactor,viewer->getRenderWindow(),interactorStyle);
        viewer->setupInteractor(interactor,viewer->getRenderWindow(),style.get());
//        interactorStyle->showCommands();
        style->showCommands();
        while(iteration_flag){
            viewer->spinOnce(100);
        }
    }
    else{
        viewer->createInteractor();
        viewer->spin();
    }
    return 0;
}
